﻿//#define POINT_MAX 10000    /*轨迹至多存储1000个点 若超过则使用最多点数*/
#ifndef PUREPURSUIT_H
#define PUREPURSUIT_H
#include <gameworld.h>
#include<algorithm>
#include<QtCore/qmath.h>
#include<qmath.h>
#include<TreeNode.h>
#include <QElapsedTimer>

class purepursuit
{
public:
    purepursuit(btosgMyCar* myVehicle);
    void PidControl(double target);
    int calc_target_index(double x[],double y[]);
    struct solutions
    {
        int ind;
        double delta;
    };
    int beforeInd=-1;
    int beforeCount=0;
    int beforeDelta=-20;
    struct solutions s;
    solutions pure_pursuit_control(double x[],double y[],int pind);
    void start_pursuit(double x[],double y[],btosgMyCar* myVehicle);
    void update(btosgMyCar* myVehicle);
    double k = 0.1;
    double Lfc = 0.1;
    double Kp = 1.0;
    double dt = 0.1;
    double L = 1.6;
    double A = 2.1;
//    double target_speed = 3.6;
    double target_speed = 30;
    double newton = 1000;
    int target_ind;
    int end = 1;
//    int POINT_MAX = 10000;
    btosgMyCar* myVehicle;
    double carx,cary,yaw,velocity;
    unsigned long long pointNum;
    //轨迹相关
    TreeNode *pTrajectory_install;		//轨迹线条安装节点
    osg::ref_ptr<osg::Node> caj;



public:
    void UpdateTraj(double *PointArray, int PointNum);
    osg::ref_ptr<osg::Node> CreateLine(double node[][3], int nodeNum, float lineWidth=5);
//public:
//    std::vector<std::vector<int>> maze;
};

#endif // PUREPURSUIT_H
